﻿#include "ObjectRecognition.h"
#include <iostream>

void ObjectRecognition::recognize(Mat& image, double length, const Mat& cameraMatrix, const Mat& distCoeffs) {
    Mat hsv;
    cvtColor(image, hsv, COLOR_BGR2HSV);

    Scalar lowerColor(100, 50, 50);
    Scalar upperColor(130, 255, 255);
    Mat mask;
    inRange(hsv, lowerColor, upperColor, mask);

    vector<vector<Point>> contours;
    findContours(mask, contours, RETR_TREE, CHAIN_APPROX_SIMPLE);

    for (const auto& contour : contours) {
        if (contourArea(contour) > 500) {
            drawContours(image, vector<vector<Point>>{contour}, -1, Scalar(0, 255, 0), 2);
            Rect rect = boundingRect(contour);
            rectangle(image, rect, Scalar(0, 255, 0), 2);

            double width = rect.width * length / rect.height;
            putText(image, format("Width: %.2f mm", width), rect.tl(), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(0, 0, 255), 2);

            // 位姿估计
            vector<Point3d> model_points;
            vector<Point2d> image_points;

            model_points.push_back(Point3d((rect.x + rect.width / 2) - 329, (rect.y + rect.height / 2) - 235, 0));
            model_points.push_back(Point3d((rect.x + rect.width / 2) - 329, (rect.y + rect.height / 2) - 235, 0));
            model_points.push_back(Point3d((rect.x + rect.width / 2) - 329, (rect.y + rect.height / 2) - 235, 0));
            model_points.push_back(Point3d((rect.x + rect.width / 2) - 329, (rect.y + rect.height / 2) - 235, 0));

            image_points.push_back(Point2d(rect.x + rect.width / 2, rect.y + rect.height / 2));
            image_points.push_back(Point2d(rect.x + rect.width / 2, rect.y + rect.height / 2));
            image_points.push_back(Point2d(rect.x + rect.width / 2, rect.y + rect.height / 2));
            image_points.push_back(Point2d(rect.x + rect.width / 2, rect.y + rect.height / 2));

            Mat rotation_vector, translation_vector;
            solvePnP(model_points, image_points, cameraMatrix, distCoeffs, rotation_vector, translation_vector);

            Mat Rvec;
            Mat_<float> Tvec;
            rotation_vector.convertTo(Rvec, CV_32F);
            translation_vector.convertTo(Tvec, CV_32F);

            Mat_<float> rotMat(3, 3);
            Rodrigues(Rvec, rotMat);

            Mat P_oc = -rotMat.inv() * Tvec;
            cout << "P_oc" << endl << P_oc << endl;
        }
    }
}

Scalar ObjectRecognition::getAverageBGRColor(const Mat& image, const vector<Point>& poly) {
    Rect rect = boundingRect(poly);
    Mat roi = image(rect);
    Scalar meanColor = mean(roi);
    return meanColor;
}

Scalar ObjectRecognition::getAverageHSVColor(const Mat& image, const vector<Point>& poly) {
    Rect rect = boundingRect(poly);
    Mat roi = image(rect);
    Mat hsvRoi;
    cvtColor(roi, hsvRoi, COLOR_BGR2HSV);
    Scalar meanColor = mean(hsvRoi);
    return meanColor;
}